#ifndef __GRIDWORLD_STATE_H__
#define __GRIDWORLD_STATE_H__

#pragma once
#include "State.h"
#include <math.h>
#include <fstream>
#include <string>

using namespace std;

class GridworldState : public State
{
private:
	int   m_col;
	int   m_row;
	int   m_size;
	bool *m_grid;
	int   m_goal;
	int   m_agent;
	int	  m_start;

public:
	GridworldState()
	{
		m_col   = 0;
		m_row   = 0;
		m_size  = 0;
		m_grid  = NULL;
		m_goal  = -1;
		m_agent = -1;
	}

	GridworldState(int col, int row)
	{
		m_col   = col;
		m_row   = row;
		m_size  = m_col * m_row;
		m_grid  = new bool[m_size];
		m_goal  = -1;
		m_agent = -1;
	}
	
	~GridworldState()
	{
		//clear();
	}
	
	void clear() {
		//*
		if (m_grid != NULL)
		{
			delete m_grid;
			m_grid = NULL;
		}
		//*/
	}

	int getIdx(int col, int row)
	{
		return row * m_col + col;
	}

	int getCol(int idx)
	{
		return idx % m_col;
	}

	int getRow(int idx)
	{
		return idx / m_col;
	}
	int getRowCount () {
		return m_row;
	}
	int getColCount () {
		return m_col;
	}
	int getSize()
	{
		return m_size;
	}

	bool setObstacle(int idx, bool obstacle)
	{
		return m_grid[idx] = obstacle;
	}

	bool isObstacle(int idx)
	{
		return m_grid[idx];
	}

	bool isObstacle(int col, int row)
	{
		return m_grid[getIdx(col,row)];
	}

	bool setStart(int start) {
		if (start >= 0 && start < m_size && !isObstacle(start))
		{
			m_start = start;
			return true;
		}
		return false;
	}
	int getStart() {
		return m_start;
	}
	bool setGoal(int goal)
	{
		if (goal >= 0 && goal < m_size && !isObstacle(goal))
		{
			m_goal = goal;
			return true;
		}
		return false;
	}

	int getGoal()
	{
		return m_goal;
	}

	bool setAgent(int agent)
	{
		if (agent >= 0 && agent < m_size && !isObstacle(agent))
		{
			m_agent = agent;
			return true;
		}
		return false;
	}

	int getAgent()
	{
		return m_agent;
	}

	virtual long getStateId()
	{
		return (StateID) getAgent();
	}

	virtual bool setFromStateId(StateID stateId)
	{
		return setAgent((int) stateId);
	}

	bool loadMap(string mapFile)
	{
		try {
			char line[2000];
			fstream fin;
			fin.open(mapFile.c_str(), fstream::in);
			
			int col = 0;
			int row = 0;

			while (!fin.eof()) {
				fin.getline(line,2000);
				vector<string> st = tokenize(line, " \t\n\r\f");
				string tag = "";
				string value = "";

				int idx = 0;
				if (idx < (int) st.size()) {			// hasMoreTokens
					tag = st.at(idx++);
					if (idx < (int) st.size()) {
						value = st.at(idx++);
					}
				}
				if (tag == "type") {
					// nothing
				}
				else if (tag == "height") {
					row = atoi(value.c_str());
				}
				else if (tag == "width") {
					col = atoi(value.c_str());
				}
				else if (tag == "map") {
					break;
				}
				else {
					printf("Error: unknown tag: %s\n", tag.c_str());
					return false;
				}
			}

			if(row <= 0 || col <= 0) {
				printf("Error: Map dimensions incorrectly specified.\n");
				fin.close();
				return false;
			}

			bool *grid = new bool[ row * col ];
			for(int r = 0 ; r < row; ++r) {
				fin.getline(line,2000);
				for(int c=0; c < col; ++c) {
					int idx = r * col + c;
					switch (line[c]) {
						case '@':
							grid[idx] = true;
							break;
						case '.':
							grid[idx] = false;
							break;
						default:
							printf("Error, unrecognized character (%c) found in map stream. Could not load map.\n", line[c]);
							fin.close();
							return false;
					}
				}
			}
			fin.close();
			m_row   = row;
			m_col   = col;
			m_size  = m_row * m_col;

			if (m_grid != NULL)
			{
				delete m_grid;
				m_grid = NULL;
			}

			m_grid  = grid;
			m_goal  = -1;
			m_agent = -1;

		} catch (iostream::failure e) {
			printf("Error: Unable to find/read mapfile: %s\n", mapFile.c_str());
			return false;
		}

		return true;
	};

	void loadMapFromPlanning( bool _grid[], int _rows, int _cols ) {
		if (m_grid != NULL) {
			delete m_grid;
			m_grid = NULL;
		}
		m_grid = _grid;
		m_size = _rows*_cols;
		m_row = _rows;
		m_col = _cols;
	};

	virtual void getActions(vector<GWAction> &actions)
	{
		int col = getCol(m_agent);
		int row = getRow(m_agent);
		bool NE=false,
			SE=false,
			SW=false,
			NW=false,
			N= false,
			E= false,
			S= false,
			W = false;

		if (row > 0)
		{
			if (col < m_col-1 && !isObstacle(col+1,row-1))
			{
				NE= true;
			}
			if (!isObstacle(col, row-1))
			{
				N = true;
			}
		}

		if (col < m_col-1)
		{
			if (!isObstacle(col+1, row))
			{
				E = true;
			}
			if (row < m_row-1 && !isObstacle(col+1, row+1))
			{
				SE=true;
			}
		}

		if (row < m_row-1)
		{
			if (!isObstacle(col, row+1))
			{
				S=true;
			}
			if (col > 0 && !isObstacle(col-1, row+1))
			{
				SW=true;
			}
		}

		if (col > 0)
		{
			if (!isObstacle(col-1, row))
			{
				W=true;
			}
			if (row > 0 && !isObstacle(col-1, row-1))
			{
				NW =true;
			}
		}
		if(NE){actions.push_back(GWAction::NE);}
		if(NW){actions.push_back(GWAction::NW);}
		if(SE){actions.push_back(GWAction::SE);}
		if(SW){actions.push_back(GWAction::SW);}
		if (N){actions.push_back(GWAction::N); }
		if (E){actions.push_back(GWAction::E); }
		if (S){actions.push_back(GWAction::S); }
		if (W){actions.push_back(GWAction::W); }
	};

	virtual void make(GWAction a)
	{
		int col = getCol(m_agent);
		int row = getRow(m_agent);

		switch (a)
		{
		case N:
			if (row > 0)
			{
				row--;
			}
			break;

		case NE:
			if (row > 0 && col < m_col-1)
			{
				row--;
				col++;
			}
			break;

		case E:
			if (col < m_col-1)
			{
				col++;
			}
			break;

		case SE:
			if (row < m_row-1 && col < m_col-1)
			{
				row++;
				col++;
			}
			break;

		case S:
			if (row < m_row-1)
			{
				row++;
			}
			break;

		case SW:
			if (row < m_row-1 && col > 0)
			{
				row++;
				col--;
			}
			break;

		case W:
			if (col > 0)
			{
				col--;
			}
			break;

		case NW:
			if (row > 0 && col > 0)
			{
				row--;
				col--;
			}
			break;
		}
		m_agent = getIdx(col, row);
	};

	virtual void retract(GWAction a)
	{
		switch (a)
		{
			case GWAction::N:
				make(GWAction::S);
				break;
			case GWAction::NE:
				make(GWAction::SW);
				break;
			case GWAction::E:
				make(GWAction::W);
				break;
			case GWAction::SE:
				make(GWAction::NW);
				break;
			case GWAction::S:
				make(GWAction::N);
				break;
			case GWAction::SW:
				make(GWAction::NE);
				break;
			case GWAction::W:
				make(GWAction::E);
				break;
			case GWAction::NW:
				make(GWAction::SE);
				break;
		}
	}

	virtual Cost getCost(GWAction a)
	{
		Cost c;
		if (a == GWAction::N || a == GWAction::E || a == GWAction::S || a == GWAction::W) {
			c = 100;
		}
		else {
			c = 150;
		}
		return c;
	}

	virtual bool isGoal()
	{
		return (m_agent == m_goal);
	}
	virtual bool isStart() {
		return m_agent == m_start;
	}
	bool contains(vector<int> ve, int val)
	{
		vector<int>::iterator it;
		for (it=ve.begin(); it<ve.end(); it++)
			if (val == (int) *it) return true;
		return false;
	}

	void draw(Solution *sol)
	{
		//printf("type tile");
		//printf("height %d\n", m_row);
		//printf("width %d\n", m_col);
		printf("map\n"); 
		int AgentLocation = getAgent();
		vector<int> loc;
		if (sol != NULL)
		{
			for (int i=0; i < (int) sol->size(); ++i) {
				loc.push_back(getAgent());
				make(sol->at(i));
			}
			/*
			for (int i=0; i < (int) sol->m_stateids.size(); ++i)
			{
				setFromStateId(sol->m_stateids.at(i));
				loc.push_back(getAgent());
			}
			setFromStateId(stateId);
			*/
		}

		for(int r=0; r<m_row; ++r)
		{
			for (int c=0; c<m_col; ++c)
			{
				if (getIdx(c,r) == getGoal()) {
					printf("G");
				}
				//else if (getIdx(c,r) == getAgent()) {
				else if (getIdx(c,r) == AgentLocation) {
					printf("A");
				}
				else if (contains(loc, getIdx(c,r))) {
					printf("x");
				}
				else if (isObstacle(c, r))
				{
					printf("@");
				}
				else
				{
					printf(".");
				}
			}
			printf("\n");
		}
	}

	virtual Cost getHeuristic()
	{
		Cost c;
		int rowDiff = abs(getRow(m_goal) - getRow(m_agent));
		int colDiff = abs(getCol(m_goal) - getCol(m_agent));

		int minDiff = min(colDiff, rowDiff);
		int maxDiff = max(colDiff, rowDiff);
		c = 150 * minDiff + 100 * (maxDiff-minDiff);
		return c;
	}
	vector<string> tokenize(const string &str, const string &delim)
	{
		vector<string> tokens;
		size_t p0 = 0, p1 = string::npos;
		while(p0 != string::npos)
		{
			p1 = str.find_first_of(delim, p0);
			if(p1 != p0)
			{
				string token = str.substr(p0, p1 - p0);
				tokens.push_back(token);
			}
			p0 = str.find_first_not_of(delim, p1);
		}
		return tokens;
	}
};
#endif // __GRIDWORLD_STATE_H__
